#include "vect_catmull.h"
#include <QDebug>

namespace catmull {
    /// <summary>
    /// Generate spline series of points from array of keyframe points
    /// </summary>
    /// <param name="points">array of key frame points</param>
    /// <param name="numPoints">number of points to generate in spline between each point</param>
    /// <returns>array of points describing spline</returns>
    void interpolar(vector<Point2f> &points, vector<Point2f> &splinePoints, int numNewPoints) {
        int i, j, numPunOri;
        Point2f punSpline;

        if (numNewPoints < 1) return;

        numPunOri = points.size();
        splinePoints.clear();

        if (numPunOri < 4) {
            for (i = 0 ; i < numPunOri ; i++) splinePoints.push_back(points[i]);
        }
        else {
            splinePoints.push_back(points[0]);

            for (i = 0 ; i < numPunOri - 3 ; i++) {
                cout << "spline en " << points[i] << points[i+1] << points[i+2] << points[i+3] << endl;
                for (j = 0 ; j < numNewPoints ; j++) {
                    punSpline = PointOnCurve(points[i], points[i + 1], points[i + 2], points[i + 3], ((float)1 / numNewPoints) * j);

                    if (punSpline.x >= 0 && punSpline.y >= 0) {
                        cout << "encolando: " << punSpline << endl;
                        splinePoints.push_back(punSpline);
                    }
                }
            }
            splinePoints.push_back(points[numPunOri - 2]);
            splinePoints.push_back(points[numPunOri - 1]);
        }
    }


    /// <summary>
    /// Calculates interpolated point between two points using Catmull-Rom Spline
    /// </summary>
    /// <remarks>
    /// Points calculated exist on the spline between points two and three.
    /// </remarks>
    /// <param name="p0">First Point</param>
    /// <param name="p1">Second Point</param>
    /// <param name="p2">Third Point</param>
    /// <param name="p3">Fourth Point</param>
    /// <param name="t">
    /// Normalised distance between second and third point
    /// where the spline point will be calculated
    /// </param>
    /// <returns>
    /// Calculated Spline Point
    /// </returns>
    Point2f PointOnCurve(Point2f p0, Point2f p1, Point2f p2, Point2f p3, float t) {
        Point2f ret;
        float t2 = t * t;
        float t3 = t2 * t;

        float long1 = sqrt((p1.x - p0.x) * (p1.x - p0.x) + (p1.y - p0.y) * (p1.y - p0.y));
        float long2 = sqrt((p3.x - p2.x) * (p3.x - p2.x) + (p3.y - p2.y) * (p3.y - p2.y));
        float diff = abs(long1 - long2);

        // Si la diferencia entre los 2 segmentos es muy grande, mejor devolver un punto nulo
        if (diff > (4 * long1) || diff > (4 * long2)) {
            ret.x = -1;
            ret.y = -1;
        }
        else {
            ret.x = 0.5f * ((2.0f * p1.x) +	(-p0.x + p2.x) * t + (2.0f * p0.x - 5.0f * p1.x + 4 * p2.x - p3.x) * t2 +
                    (-p0.x + 3.0f * p1.x - 3.0f * p2.x + p3.x) * t3);
            ret.y = 0.5f * ((2.0f * p1.y) + (-p0.y + p2.y) * t + (2.0f * p0.y - 5.0f * p1.y + 4 * p2.y - p3.y) * t2 +
                    (-p0.y + 3.0f * p1.y - 3.0f * p2.y + p3.y) * t3);
        }

        return ret;
    }
}
